//
// Created by milk on 24-7-6.
//

#ifndef SHENZHOU_BIANDUI_QUADROTOR_H
#define SHENZHOU_BIANDUI_QUADROTOR_H

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <atomic>
#include<geometry_msgs/PoseStamped.h>
#include<sensor_msgs/NavSatFix.h>
#include<geometry_msgs/TwistStamped.h>
#include<geometry_msgs/Point.h>
#include<std_msgs/UInt64.h>
#include <std_msgs/Bool.h>
#include<sensor_msgs/PointCloud2.h>
#include <mavros_msgs/Altitude.h>
#include <octomap/octomap.h>
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>
#include "unionsys_core.hpp"
//sender
#include <iostream>
#include <cstring>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <vector>
#include <unistd.h>

extern RegisterMSN *reg_pub;
extern RegisterMSN *reg_sub;

#define user_QuadrotorNum 5

class PoseMsg {
public:
    void deserialize(const char *data, size_t length);

    std::vector<char> serialize() const;

    std::string seq;
    std::string frame_id;
    std::string xPoint, yPoint, zPoint;
    std::string xOrientation, yOrientation, zOrientation, wOrientation;

};

class Quadrotor {
public:
    void set_id(int id);

    int get_num();

    int get_id();

    void rosstateloopdata(const mavros_msgs::State::ConstPtr &state);

    void rosrcinloopdata(const mavros_msgs::RCIn::ConstPtr &RC_in);

    void rosvelloopdata(const geometry_msgs::TwistStamped::ConstPtr &vel_local);

    void rosgpsloopdata(const sensor_msgs::NavSatFix::ConstPtr &gps);

    void rosaltitudeloopdata(const mavros_msgs::Altitude::ConstPtr &msg);

    static const char *ipAddresses[];

    geometry_msgs::Point self_gps_point;

    int channel5_value = 0;
    int channel6_value = 0;
    int channel7_value = 0;
    int channel8_value = 0;

    int formation_number = 1;
    double current_vel_x = 0.0;
    double current_vel_y = 0.0;
    double current_vel_z = 0.0;

    double current_point_z = 0.0;

    mavros_msgs::State current_state;

private:

    int QuadrotorNum = user_QuadrotorNum;

    int QuadrotorId;
};


#endif //SHENZHOU_BIANDUI_QUADROTOR_H
